#include "MyMessage.h"
#include "Timer.h"

module routeDiscoveryC{

  uses{
    interface Boot;
    interface AMPacket;
    interface Packet;
    interface PacketAcknowledgements;
    interface AMSend;
    interface SplitControl;
    interface Receive;
    interface Timer<TMilli> as MilliTimer;
  }
}

implementation{
    uint8_t i_c;				// UN CONTATORE PER LA FORWARD...E PER TENERE LE ROUTE..
	uint8_t route_size=50;
    message_t packet;
    message_t packet_sendRREP;		
    message_t packet_forwardRREQ;	
    message_t packet_RREPreceived;	
    message_t packet_forwardRREP;	
    messaggio_t* rpack;
    messaggio_t* rpack_sendRREP;	
    messaggio_t* rpack_forwardRREQ;	
    messaggio_t* rpack_RREPreceived;	
    messaggio_t* rpack_forwardRREP;	
   
    enum {ARRAY_SIZE = 100};
	
    /*teniamo in memoria l'hop count per ogni pacchetto ricevuto destinato ad un certo nodo.
	Per ogni destinazione si fa il forward del pacchetto solo se il suo hop count è minore (<=) del
	valore già memorizzato*/
    uint8_t hop_counts[ARRAY_SIZE];  

    
    //***************** Interfaccia boot ********************//
	//IL NODO 7 DICHIARA DI ESSERE ACCESO
    event void Boot.booted(){
	uint8_t i;
	for (i=0;i<ARRAY_SIZE;i++){
	    hop_counts[i]=ARRAY_SIZE;
	}    
	if (TOS_NODE_ID==7){
	    dbg("boot","Sono acceso 		%s\n",sim_time_string());
		}
	call SplitControl.start();
	
    }

	
    //***************** Task send request ********************//
	//SCATENA LA RICHIESTA DI ROUTE VERSO IL NODO DESTINAZIONE. VA IN BROADCAST
    task void sendRREQ() {
	uint8_t len;						
	// Scateno la Route Request per il nodo #
	messaggio_t* mess=(messaggio_t*)(call Packet.getPayload(&packet,&len));
	mess->msg_type=0;					// Tipo di messaggio
	mess->source_id=TOS_NODE_ID;		// L identificativo della sorgente
	mess->dest_id=81;					// identificativo della destinazione
	mess->hop_count=1;					// inizio da uno nell hop count per semplicità della gestione
	mess->route[0]=TOS_NODE_ID;			// metto nella route l identificativo del nodo
	hop_counts[TOS_NODE_ID]=0;			// da me disto 0
	//dbg("tx","Sto mandando RREQ... 		%s\n",sim_time_string());
	call PacketAcknowledgements.noAck( &packet );
	if(call AMSend.send(TOS_BCAST_ADDR,&packet,sizeof(messaggio_t)) == SUCCESS){
	    dbg("tx","Scatenata richiesta di route...destinazione %i. Trasmetto in broadcast		%s\n",mess->dest_id,sim_time_string());
	}
	else {
	    dbg("tx","Non son riuscito a mandare la RREQ 		%s\n",mess->dest_id,sim_time_string());
	}
	
    } 

	
    //***************** Interfaccia TMilli ********************//
	//TIMER PER SCATENARE LA RICHIESTA DI ROUTE
    event void MilliTimer.fired() {
 	post sendRREQ();
    }

	
   	//***************** Interfaccia SplitControl ********************//
	//CONTROLLO DELLA RADIO
    event void SplitControl.startDone(error_t err){
	if(err == SUCCESS){
	    if (TOS_NODE_ID==7){
		post sendRREQ(); // PROVA PER NON ATTENDERE IL PRIMO PERIODIC
		call MilliTimer.startPeriodic( 5000 ); // CI METTE MENO DI 1000 PER FAR TORNARE INDIETRO LA RISPOSTA SU UNA GRIGLIA 10X10
		} 
	}
	else{
	    call SplitControl.start();
	}
    }
  
    event void SplitControl.stopDone(error_t err){}
	
	
	//***************** Task forward RREQ ********************//
	//ESEGUE LA RICHIESTA DI FORWARD IN AVANTI DEI PACCHETTI. LO TRASMETTE IN BROADCAST
	
    task void forwardRREQ() {
	nx_uint8_t count;					// contatore di nodi attraversati
	nx_uint16_t dest;					// nodo di destinazione del pacchetto
	uint8_t len;					
	messaggio_t* mess_f=(messaggio_t*)(call Packet.getPayload(&packet_forwardRREQ,&len));	
	count=rpack_forwardRREQ->hop_count;			// leggo l hop count che trovo nel pacchetto
	dest=rpack_forwardRREQ->dest_id;
	//dbg("tx","son nella forward, ho attraversato: %i nodi		%s\n",rpack_forwardRREQ->hop_count,sim_time_string()); 
	rpack_forwardRREQ->route[count]=TOS_NODE_ID;	// aggiungo l'identificativo del nodo nel pacchetto
	rpack_forwardRREQ->hop_count=count+1;		// aggiunge uno all hop count
	hop_counts[dest]=count;				// memorizza la metrica in hop verso il nodo dest
	
	// passaggio da messaggio ricevuto a messaggio da forwardare
	mess_f->msg_type=rpack_forwardRREQ->msg_type;
	mess_f->source_id=rpack_forwardRREQ->source_id;
	mess_f->dest_id=rpack_forwardRREQ->dest_id;
	mess_f->hop_count=rpack_forwardRREQ->hop_count;
	for (i_c=0;i_c<route_size;i_c++){
	    mess_f->route[i_c]=rpack_forwardRREQ->route[i_c];
	}
	
	//qualche controllo
	dbg("tx","Forwarding in avanti.ho attraversato %i nodi..sto andando verso %i 		%s\n",rpack_forwardRREQ->hop_count,rpack_forwardRREQ->dest_id,sim_time_string());
	//dbg("tx","Forwarding RREQ...sto andando verso DA MESS_f %i 		%s\n",mess_f->dest_id,sim_time_string());
	
	call PacketAcknowledgements.noAck( &packet_forwardRREQ ); // manda il pacchetto modificato
	if(call AMSend.send(TOS_BCAST_ADDR,&packet_forwardRREQ,sizeof(messaggio_t)) == SUCCESS){
	//dbg("tx","Mandata Forwarding RREQ...sto andando verso da rpack %i 		%s\n",rpack_forwardRREQ->dest_id,sim_time_string());
	//dbg("tx","Mandata Forwarding RREQ...sto andando verso DA MESS_f %i		%s\n",mess_f->dest_id,sim_time_string()); 	
	}
	else {dbg("tx","Non sono riuscito a mandare la forwardRREQ 		%s\n",sim_time_string());}
    } 

	
    //***************** Task send route response ********************//
	//GENERA LA RISPOSTA E LA INVIA ALL ULTIMO HOP RICEVUTO. NON IN BROADCAST. ESEGUE CONTROLLO SULL ACK
    task void sendRREP() {
	nx_uint8_t count;					// contatore di nodi attraversati
	nx_uint16_t dest;					// nodo di destinazione del pacchetto
	nx_uint8_t source;
	nx_uint8_t source_count;
	uint8_t len;
	messaggio_t* mess_r=(messaggio_t*)(call Packet.getPayload(&packet_sendRREP,&len));
	source=rpack_sendRREP->source_id;
	source_count = hop_counts[source];
	count=rpack_sendRREP->hop_count;
	if (count <= source_count){	// QUI FACCIO IL CONTROLLO PER TRASMETTERE SOLO LE ROUTE PIù BREVI
	    
	    hop_counts[rpack_sendRREP->source_id]=count;
	    dbg("tx","RICHIESTA RICEVUTA.STO FORGIANDO LA RISPOSTA\n");	    
	    rpack_sendRREP->route[count]=TOS_NODE_ID;	// aggiungo l identificativo del nodo nel pacchetto
	    
		mess_r->msg_type=1;
	    mess_r->hop_count=rpack_sendRREP->hop_count;
	    mess_r->source_id=TOS_NODE_ID;
	    mess_r->dest_id=rpack_sendRREP->source_id;
	    
	    for (i_c=0;i_c<(mess_r->hop_count)+1;i_c++){
		mess_r->route[i_c]=rpack_sendRREP->route[(mess_r->hop_count)-(i_c)];
		    //mess_r->route[i_c]=rpack_sendRREP->route[(i_c)]; 		//così sarebbe dritto invece la giro
		    dbg("tx","mess_r->route[i_c%i]=%i \n",i_c,mess_r->route[i_c]);
	    }
	    dbg("tx","hop count %i\n",mess_r->hop_count);
	    mess_r->hop_count=1;
	    //qualche controllo
	    dest=rpack_sendRREP->route[(rpack_sendRREP->hop_count)-1];	// estrae l'indirizzo del prossimo nodo nel percorso di ritorno	
	    //dbg("tx","Sending RREP... Vado verso %i 		%s\n",dest,sim_time_string());
	    
		call PacketAcknowledgements.requestAck( &packet_sendRREP ); // manda il pacchetto modificato
		
	    if(call AMSend.send(dest,&packet_sendRREP,sizeof(messaggio_t)) == SUCCESS){
		    dbg("tx","Trasmesso pacchetto di risposta...sto andando verso %i		%s\n",dest,sim_time_string());
			}
	    else {
		dbg("tx","non sono riuscito a mandare la sendRREP 		%s\n",sim_time_string());
	    }
		
	}
	else{
	    dbg("tx","count: %i\n",count);
	    dbg("tx","source_count: %i\n",source_count);
	    dbg("tx","Pacchetto proveniente da percorso non ottimo....non invio RREP\n");
	}
	
    } 
	
	
	//***************** Task REsend route response ********************//
	/*Funzione chiamata da sendDone nel caso in cui non venga ricevuto l'ack per il send della sendRREP*/
    task void REsendRREP() {
	nx_uint16_t dest;						// nodo di destinazione del pacchetto
	dest=rpack_sendRREP->route[(rpack_sendRREP->hop_count)-1];	// estrae l'indirizzo del prossimo nodo nel percorso di ritorno	
	
	dbg("tx","REsending sendRREP...il prossimo hop è %i 		%s\n",dest,sim_time_string());
	
	call PacketAcknowledgements.requestAck( &packet_sendRREP ); // manda il pacchetto modificato
	
		if(call AMSend.send(dest,&packet_sendRREP,sizeof(messaggio_t)) == SUCCESS){
		    dbg("tx","Trasmesso pacchetto di risposta...sto andando verso %i		%s\n",dest,sim_time_string());
			}
	    else {
		dbg("tx","non sono riuscito a mandare la sendRREP 		%s\n",sim_time_string());
	    }
		
	}
	
	
    //***************** Task forward RREP ********************//
	//FORWARD DELLA RISPOSTA. TRASMESSA NON IN BROADCAST MA DIRETTAMENTE. FA CONTROLLO SULL ACK
    task void forwardRREP() {
	
	nx_uint8_t count;					// contatore di nodi attraversati
	nx_uint16_t dest;					// nodo di destinazione del pacchetto
	uint8_t len;	
	
	messaggio_t* mess_rrep=(messaggio_t*)(call Packet.getPayload(&packet_forwardRREP,&len));
	
	count=rpack_forwardRREP->hop_count;				// leggo l hop count che trovo nel pacchetto
	
	dbg("tx","son nella forward RREP, ho attraversato: %i nodi		%s\n",rpack_forwardRREP->hop_count,sim_time_string()); 
	
	rpack_forwardRREP->hop_count=count+1;			// aggiunge uno all hop count
	hop_counts[dest]=count;							// matrice per vedere la distanza tra sorgente e il nodo ricevente DA VERIFICARE LA MATRICE!!! PER ORA E' UN CONTATORE NON TROPPO SIGNIFICATIVO TRA LA SORGENTE ED IL NODO RICEVENTE
	
	// passaggio da messaggio ricevuto a messaggio da forwardare
	mess_rrep->msg_type=rpack_forwardRREP->msg_type;
	mess_rrep->source_id=rpack_forwardRREP->source_id;
	mess_rrep->dest_id=rpack_forwardRREP->dest_id;
	mess_rrep->hop_count=rpack_forwardRREP->hop_count;
	dest=rpack_forwardRREP->route[(rpack_forwardRREP->hop_count)];
	
	for (i_c=0;i_c<route_size;i_c++){
	    mess_rrep->route[i_c]=rpack_forwardRREP->route[i_c];
		//dbg("tx","mess_r->route[i_c%i]=%i \n",i_c,mess_rrep->route[i_c]);			//	TOLTO ALTRIMENTI VISUALIZZA TUTTO IL VETTORE ROUTE
	}
	
	//qualche controllo
	
	dbg("tx","Forwarding RREP...il prossimo hop è %i 		%s\n",dest,sim_time_string());
	//dbg("tx","Forwarding RREP...msg_type %i 		%s\n",mess_rrep->msg_type,sim_time_string());
	//dbg("tx","Forwarding RREP...source_id %i 		%s\n",mess_rrep->source_id,sim_time_string());
	//dbg("tx","Forwarding RREP...dest_id %i 		%s\n",mess_rrep->dest_id,sim_time_string());
	dbg("tx","Forwarding RREP...hop_count %i 		%s\n",mess_rrep->hop_count,sim_time_string());
	//dbg("rx","SIMULAZIONE DI RICEZIONE della Forwarding RREP, Ricevuto un pacchetto da: %i 		%s\n",mess_rrep->route[(mess_rrep->hop_count)-1],sim_time_string()); 
		
	call PacketAcknowledgements.requestAck( &packet_forwardRREP ); // manda il pacchetto modificato
	if(call AMSend.send(dest,&packet_forwardRREP,sizeof(messaggio_t)) == SUCCESS){
	//dbg("tx","Mandata Forwarding RREP...sto andando verso da mess_rrep%i 		%s\n",mess_rrep->dest_id,sim_time_string());
	//dbg("tx","Mandata Forwarding RREQ...sto andando verso DA MESS_f %i 		%s\n",mess_f->dest_id,sim_time_string());
	}
	else {dbg("tx","non sono riuscito a mandare la forwardRREP 		%s\n",sim_time_string());}
	
    } 
	
	
	//***************** Task REforward RREP ********************//
	/*Funzione chiamata da sendDone nel caso in cui non venga ricevuto l'ack per il forward di una RREP*/

	task void REforwardRREP() {
	nx_uint16_t dest;					// nodo di destinazione del pacchetto
	
	dest=rpack_forwardRREP->route[(rpack_forwardRREP->hop_count)];
	
	//qualche controllo
	
	dbg("tx","REForwarding forwardRREP...il prossimo hop è %i 		%s\n",dest,sim_time_string());
	
	//dbg("tx","Forwarding RREP...msg_type %i 		%s\n",mess_rrep->msg_type,sim_time_string());
	//dbg("tx","Forwarding RREP...source_id %i 		%s\n",mess_rrep->source_id,sim_time_string());
	//dbg("tx","Forwarding RREP...dest_id %i 		%s\n",mess_rrep->dest_id,sim_time_string());
	//dbg("tx","Forwarding RREP...hop_count %i 		%s\n",mess_rrep->hop_count,sim_time_string());
	//dbg("rx","SIMULAZIONE DI RICEZIONE della Forwarding RREP, Ricevuto un pacchetto da: %i 		%s\n",mess_rrep->route[(mess_rrep->hop_count)-1],sim_time_string()); 
		
	call PacketAcknowledgements.requestAck( &packet_forwardRREP ); // manda il pacchetto modificato
	if(call AMSend.send(dest,&packet_forwardRREP,sizeof(messaggio_t)) == SUCCESS){
	
	//dbg("tx","Mandata Forwarding RREP...sto andando verso da mess_rrep%i 		%s\n",mess_rrep->dest_id,sim_time_string());
	//dbg("tx","Mandata Forwarding RREQ...sto andando verso DA MESS_f %i 		%s\n",mess_f->dest_id,sim_time_string());
	}
	else {dbg("tx","non sono riuscito a mandare la forwardRREP 		%s\n",sim_time_string());}
	
    } 
	
	
	//***************** Task RREPreceived********************//
	//PACCHETTO RICEVUTO. VISUALIZZA LA ROUTE
	task void RREPreceived() { 		dbg("rx","ricevuto il messaggio di risposta 		%s\n",sim_time_string());
	//rpack_RREPreceived
	for (i_c=0;i_c<(rpack_RREPreceived->hop_count)+1;i_c++){
	    //mess_rrep->route[i_c]=rpack_forwardRREP->route[i_c];
		dbg("tx","yeah arrivato! la route totale è mess_r->route[i_c%i]=%i \n",i_c,rpack_RREPreceived->route[i_c]);
	}
	call MilliTimer.stop(); // FERMA L INVIO DELLA SEND del nodo d origine
	// Ricevuto il messaggio di risposta dal nodo che ha scatenato la request
	}



    //********************** Interfaccia Send ****************//
    //CONTROLLO DELLO STATO DEL SEND. SI POSSONO EFFETTUARE VARI CONTROLLI

    event void AMSend.sendDone(message_t* buf,error_t err){
	//messaggio_t* check = (messaggio_t*)(buf->data);	//PASSA IL CONTENUTO DI BUF NEL CHECK. DA QUI SI POSSONO FARE VARI CONTROLLI.
	//dbg("tx","SONO NELLA SEND e sto andando verso %i 		%s\n",check->dest_id,sim_time_string());	//la destinazione finale è dest id ma non è detto che sia il prossimo hop
	if(&packet == buf && err == SUCCESS ){
	    if ( TRUE ) {	//messaggio_t* check = (messaggio_t*)(buf->data);
						//dbg("tx","Sono nella send.Ho mandato pacchetto.. sto andando verso %i 		%s\n",check->dest_id,sim_time_string());
	    } 
	    else {
		dbg("tx","Invio iniziale fallito 		%s\n",sim_time_string());
		}
	}
	if(&packet_forwardRREQ == buf && err == SUCCESS ){
	    if ( TRUE ) {	//messaggio_t* check = (messaggio_t*)(buf->data);
						//dbg("tx","Sono nella send.Ho mandato pacchetto della forwardRREQ.. sto andando verso %i 		%s\n",check->dest_id,sim_time_string());
	    } 
	    else {
		dbg("tx","Invio forward fallito 		%s\n",sim_time_string());
		}
	} 
	if(&packet_sendRREP == buf && err == SUCCESS ){
	    if ( call PacketAcknowledgements.wasAcked( buf ) ) {
		dbg("tx","inviata correttamente sendRREP\n");
		}
	    else {
		dbg("tx","fallito provo a ripostare \n");
		post REsendRREP();
	    }
	}
	if(&packet_forwardRREP == buf && err == SUCCESS ){
	    if ( call PacketAcknowledgements.wasAcked( buf ) ) {
		dbg("tx","inviata correttamente forwardRREP\n");
		} 
	    else {
		dbg("tx","fallito provo a ripostare \n");
	        post REforwardRREP();
	        }
	    }
		
	}


    //***************************** Interfaccia Receive *****************//
    /*Funzione che discrimina i vari i casi in cui ci si può trovare al momento della ricezione di un pacchetto*/

    event message_t* Receive.receive(message_t* buf,void* payload, uint8_t len){
	messaggio_t* mess=(messaggio_t*)(payload);
	
	rpack = mess;
	dbg("rx","Ricevuto un pacchetto da: %i 		\n",rpack->route[(rpack->hop_count)-1]); 
	//prima di tutto distinguiamo tra una RREQ e una RREP
	if ( rpack->msg_type == RREQ ) {
	    //il pacchetto è una RREQ (richiesta di route)
	    //se il pacchetto è giunto a destinazione il nodo manda una RREP
	    if ( rpack->source_id == TOS_NODE_ID ) { //TORNATA AL NODO SORGENTE. UCCIDE IL PACCHETTO
		dbg("rx","Sono Io che ho scatenato la richiesta...non deve passare da me ancora.. Pacchetto scartato\n");
		return buf;
	    }
		if ( rpack->dest_id == TOS_NODE_ID ) { //ARRIVATA AL NODO DESTINAZIONE. FORGIA LA RISPOSTA
		rpack_sendRREP=rpack;
		post sendRREP();
		//dbg("rx","Postata la sendRREP 		%s\n",sim_time_string()); 
		return buf;
	    }
	    //altrimenti se il pacchetto ha un hop count maggiore di quello che il
	    //nodo ha già memorizzato per la destinazione, il pacchetto viene scartato.
	    //Se invece l'hop count è minore o uguale viene forwardato
	    else {
		if ( rpack->hop_count > hop_counts[rpack->dest_id] ) {
		    //caso del pacchetto scartato
		    dbg("rx","Pacchetto scartato,percorso troppo lungo %i > %i		%s\n",rpack->hop_count,hop_counts[rpack->dest_id],sim_time_string()); 
			return buf;
		}
		else {
		    //caso del pacchetto forwardato
		    rpack_forwardRREQ=rpack;
			post forwardRREQ();
			dbg("rx","Pacchetto forwardato, trasmesso in broadcast destinato a %i 		%s\n",rpack_forwardRREQ->dest_id,sim_time_string()); 
			return buf;
		}
	    }
	}	
	else {
	    //il messaggio è di tipo RREP
		dbg("rx","Questa è una Reply, la destinazione finale è %i		%s\n",rpack->dest_id,sim_time_string());
	    //dbg("rx","Questa è una Reply, rpack->route[rpack->hop_count] è %i		%s\n",rpack->route[rpack->hop_count],sim_time_string());
		if ( rpack->dest_id == rpack->route[rpack->hop_count] ) {
		//RREP è giunta al nodo sorgente iniziale
		//dbg("rx","Prima di lanciare la ricezione finale 		%s\n",sim_time_string()); 
		rpack_RREPreceived=rpack;
		post RREPreceived();
		dbg("rx","Arrivata al nodo iniziale 		%s\n",sim_time_string()); 
		return buf;
	    }
	    else {
		//RREP non è ancora giunta a destinazione e viene forwardata indietro
		//dbg("rx","Sto per forwardare verso il nodo %i 		%s\n",rpack->route[(rpack->hop_count)+1],sim_time_string()); 
		rpack_forwardRREP=rpack;
		post forwardRREP();
		dbg("rx","Forwardata verso il nodo sorgente 		%s\n",sim_time_string()); 
		return buf;
	    }
	}
	dbg("rx","problema in ricezione?");
	return buf;
	}
	
}
